#include <intersectionConvexPolygons.h>
#include <szMiscOperations.h>
//#include <DotGroupUtility.h>
#include <szMexUtility.h>
#include <cassert>
namespace _IntersectConvexPolygon
{
	/*
	*/
	bool
	inside(const CParticleF& p, const vector<CParticleF>& pnts)
	{
		bool c = false;
		int x = p.m_X;
		int y = p.m_Y;
		for (int i = 0, j = pnts.size()-1; i < pnts.size(); j = i++) 
		{
			float xi = pnts[i].m_X;
			float yi = pnts[i].m_Y;
			float xj = pnts[j].m_X;
			float yj = pnts[j].m_Y;
			if ( ((yi>y) != (yj>y)) &&
				(x < (xj-xi) * (y-yi) / (yj-yi) + xi) )
				c = !c;
		}
		return c;
	}

	/*
	find an intersection between two line segments defined by [p1 - p2] and [q1 - q2].
	It returns a parameter s such that the intersection point is (1-s)*p1 + s*p2.
	*/
	float
	intersect(CParticleF& p1, CParticleF& p2, CParticleF& q1, CParticleF& q2)
	{
		float x0=p1.m_X;
		float y0=p1.m_Y;
		float x1=p2.m_X;
		float y1=p2.m_Y;
		float x2=q1.m_X;
		float y2=q1.m_Y;
		float x3=q2.m_X;
		float y3=q2.m_Y;
		float det = (x0-x1)*(y2-y3) - (x2-x3)*(y0-y1);
		if(Abs(det) < std::numeric_limits<float>::epsilon()* 10)
		{
			return std::numeric_limits<float>::quiet_NaN();
		}
		float s = ((y2-y3)*(x0-x2)+(x3-x2)*(y0-y2))/det;
		float t = ((y1-y0)*(x0-x2)+(x0-x1)*(y0-y2))/det;
		return s;
	}

	/*
	Pre-condition: p is inside a polygon defined by points, and q is outside the polygon.
	Find a segment of the polygon that intersects [p - q].
	Returns the first index of the point for the intersection segment, and the parameter t
	that gives the exact point of the intersection.
	The exact intersection is (1-t)*p + t*q.
	*/
	pair<int,float> 
	findIntersectionSegment(CParticleF& p, CParticleF& q, std::vector<CParticleF>& points)
	{
		pair<int,float> res;
		res.first = points.size();
		res.second = std::numeric_limits<float>::quiet_NaN();
		for(int i=0; i<points.size(); ++i)
		{
			int j = i==points.size()-1 ? 0: i+1;
			float t = intersect(p, q, points[i], points[j]);
			if(t >=0 && t<1.0f)
			{
				res.first = i;
				res.second = t;
				break;
			}
		}
		return res;
	}

	/*
	return false if a point in P is inside Q AND a point in Q is inside P.
	*/
	bool
	noIntersection(std::vector<CParticleF>& P, 
						 std::vector<CParticleF>& Q)
	{
		bool b1 = false;
		for(int i=0; i<P.size(); ++i)
		{
			if(inside(P[i], Q))
			{
				b1 = true;
				break;
			}
		}
		bool b2 = false;
		for(int i=0; i<Q.size(); ++i)
		{
			if(inside(Q[i], P)) 
			{
				b2 = true;
				break;
			}
		}
		if(b1 && b2) return false;
		else return true;
	}
	bool
	containsTheOther(std::vector<CParticleF>& P, 
						 std::vector<CParticleF>& Q)
	{
		for(int i=0; i<Q.size(); ++i)
		{
			if(inside(Q[i], P)==false) return false;
		}
		return true;
	}

	bool
	checkSwitch(vector<CParticleF>& cap, vector<CParticleF>& P, vector<CParticleF>& Q, int id)
	{
		cap.push_back(P[id]);
		int id2 = (id + 1) % P.size();
		bool bSwitch = false;
		if(inside(P[id2], Q)==false)
		{
			//find the intersection point
			pair<int,float> seg = findIntersectionSegment(P[id], P[id2], Q);
			CParticleF x = Q[seg.first];
			CParticleF y = Q[(seg.first+1) % Q.size()];
			float t = seg.second;
			CParticleF z(x.m_X*(1-t)+y.m_X*t, x.m_Y*(1-t)+y.m_Y*t);
			cap.push_back(z);
			if(inside(y,P)==false)
			{
				bSwitch = true;
				id = (seg.first+1) % Q.size();
			}
		}
		return bSwitch;
	}
	/*
	a brute force approach to finding the intersection of two convex polygons.
	1. find a point in P that is inside Q
	2. find two segments in P and Q that intersect
	3. construct a polygon from the two intersection points.
	*/
	std::vector<CParticleF> 
	intersectConvexHulls(std::vector<CParticleF>& P, 
						 std::vector<CParticleF>& Q)
	{
		if(noIntersection(P, Q))
		{
			return std::vector<CParticleF>();
		}
		else if(containsTheOther(P, Q))
		{
			return Q;
		}
		else if(containsTheOther(Q, P))
		{
			return P;
		}
		else
		{
			std::vector<CParticleF> cap;
			int id = -1;
			bool bStart = true;
			for(int i=0; i<P.size(); ++i)
			{
				if(inside(P[i],Q))
				{
					id = i;
					break;
				}
			}
			assert(id>=0); //since we are in the else clause, there has to be an intersection
			int first = id;
			bool bPactive = bStart;
			do
			{
				if(bPactive)
				{
					cap.push_back(P[id]);
					int id2 = (id + 1) % P.size();
					if(inside(P[id2], Q)==false)
					{
						//find the intersection point
						pair<int,float> seg = findIntersectionSegment(P[id], P[id2], Q);
						CParticleF x = Q[seg.first];
						CParticleF y = Q[(seg.first+1) % Q.size()];
						float t = seg.second;
						CParticleF z(x.m_X*(1-t)+y.m_X*t, x.m_Y*(1-t)+y.m_Y*t);
						cap.push_back(z);
						bPactive = false;
						id = (seg.first+1) % Q.size();
					}
					else
					{
						id = id2;
					}
				}
				else
				{
					cap.push_back(Q[id]);
					int id2 = (id + 1) % Q.size();
					if(inside(Q[id2], P)==false)
					{
						//find the intersection point
						pair<int,int> seg = findIntersectionSegment(Q[id], Q[id2], P);
						CParticleF x = P[seg.first];
						CParticleF y = P[(seg.first+1) % P.size()];
						float t = seg.second;
						CParticleF z(x.m_X*(1-t)+y.m_X*t, x.m_Y*(1-t)+y.m_Y*t);
						cap.push_back(z);
						bPactive = true;
						id = (seg.first+1) % P.size();
					}
					else
					{
						id = id2;
					}
				}
			}
			while(!(bPactive==bStart && id==first));
			return cap;
		}
	}
}
